#! /usr/bin/env roseus
;; -*- mode: EusLisp; coding: utf-8-unix; -*-
#-:jsk
(jsk)
#-:rbrain
(rbrain)

(defun make-irt-all-objects-3dp (&key (save-img nil) (use-poisson t) (normal t) (dump nil))
  (load (format nil "~A/jskeus/eus/models/load-irt-all-objects.l"
                (read (piped-fork "rospack find euslisp"))))
  (send *pickview* :resize 640 480)
  (send *viewer* :viewsurface :bg-color #f(0.3 0.3 0.3 0))
  (send *viewer* :draw-objects)

  (setq *irt-all-objects-3dp*
        (mapcar
         #'(lambda (x)
             (let* ((mdl (eval x))
                    (3dp (if use-poisson
                             (make-3dpointcloud-on-object-poisson mdl 5.0 :normal normal)
                           (make-3dpointcloud-on-object mdl))))
               (send 3dp :name (send mdl :name))
               (when dump (dump-structure (format nil "~A-3dp.l" (send 3dp :name)) 3dp))
               3dp))
         ;;*irt-all-objects*))
         (subseq *irt-all-objects* 4)))
  (when save-img
    (dolist (3dp *irt-all-objects-3dp*)
      (objects (list 3dp))
      (send *viewer* :viewsurface :write-to-ppm-file (format nil "~A.ppm" (send 3dp :name)))))
  )

(defun make-irt-objects-3dp-from-name (model-name
                                       &key (return-object nil)
                                       (save-img nil)
                                       (use-poisson t)
                                       (r 5.0)
                                       (normal t))
  (load (format nil "~A/jskeus/eus/models/~A-object.l"
                (read (piped-fork "rospack find euslisp"))
                model-name))
  (let* ((mdl (eval
               (read-from-string
                (format nil "(~A)" model-name))))
         (3dp (if use-poisson
                  (make-3dpointcloud-on-object-poisson mdl r :normal normal)
                (make-3dpointcloud-on-object mdl r 1.0 :normal normal))))
    (send 3dp :name (send mdl :name))
    (objects (list 3dp))
    (when save-img
      (send *viewer* :viewsurface :write-to-ppm-file (format nil "~A.ppm" (send 3dp :name))))
    (if return-object (list mdl 3dp) 3dp)
    ))